using System;
using System.Collections.Generic;
using Engine;

namespace RuthlessConquest {
    public abstract class AStar<T> {
        public class Node {
            public T Position;

            public T PreviousPosition;

            public int F;

            public int G;

            public int H;

            public bool IsClosed;
        }

        public int NodesCacheIndex;

        public DynamicArray<Node> NodesCache = new();

        public DynamicArray<Node> OpenHeap = new();

        public DynamicArray<T> NeighborsList = new();

        public Dictionary<T, object> DefaultStorage;

        public abstract void Neighbors(T p, DynamicArray<T> neighbors);

        public abstract int Cost(T p1, T p2);

        public abstract int Heuristic(T p);

        public virtual void StorageClear() {
            if (DefaultStorage == null) {
                DefaultStorage = new Dictionary<T, object>();
            }
            else {
                DefaultStorage.Clear();
            }
        }

        public virtual object StorageGet(T p) {
            DefaultStorage.TryGetValue(p, out object value);
            return value;
        }

        public virtual void StorageAdd(T p, object data) {
            DefaultStorage.Add(p, data);
        }

        public bool FindPath(ICollection<T> path, T start, int maxPositionsToCheck = int.MaxValue) {
            if (path == null) {
                throw new ArgumentNullException("path");
            }
            path.Clear();
            NodesCacheIndex = 0;
            OpenHeap.Clear();
            StorageClear();
            Node node = NewNode(start, default, 0, 0);
            StorageAdd(start, node);
            HeapEnqueue(node);
            Node node2 = null;
            int num = 0;
            Node node3;
            while (true) {
                node3 = OpenHeap.Count > 0 ? HeapDequeue() : null;
                if (node3 == null
                    || num >= maxPositionsToCheck) {
                    if (node2 != null) {
                        BuildPathFromEndNode(path, node, node2);
                    }
                    return false;
                }
                if (Heuristic(node3.Position) <= 0) {
                    break;
                }
                if (node3 != node
                    && (node2 == null || node3.H < node2.H)) {
                    node2 = node3;
                }
                node3.IsClosed = true;
                num++;
                NeighborsList.Clear();
                Neighbors(node3.Position, NeighborsList);
                for (int i = 0; i < NeighborsList.Count; i++) {
                    T val = NeighborsList[i];
                    Node node4 = (Node)StorageGet(val);
                    if (node4 != null
                        && node4.IsClosed) {
                        continue;
                    }
                    int num2 = Cost(node3.Position, val);
                    if (num2 == int.MaxValue) {
                        continue;
                    }
                    int num3 = node3.G + num2;
                    if (node4 != null) {
                        if (num3 < node4.G) {
                            node4.G = num3;
                            node4.F = num3 + node4.H;
                            node4.PreviousPosition = node3.Position;
                            HeapUpdate(node4);
                        }
                    }
                    else {
                        int h = Heuristic(val);
                        node4 = NewNode(val, node3.Position, num3, h);
                        StorageAdd(val, node4);
                        HeapEnqueue(node4);
                    }
                }
            }
            BuildPathFromEndNode(path, node, node3);
            return true;
        }

        public void BuildPathFromEndNode(ICollection<T> path, Node startNode, Node endNode) {
            for (Node node = endNode; node != startNode; node = (Node)StorageGet(node.PreviousPosition)) {
                path.Add(node.Position);
            }
        }

        public void HeapEnqueue(Node node) {
            OpenHeap.Add(node);
            HeapifyFromPosToStart(OpenHeap.Count - 1);
        }

        public Node HeapDequeue() {
            Node result = OpenHeap[0];
            if (OpenHeap.Count <= 1) {
                OpenHeap.Clear();
                return result;
            }
            OpenHeap[0] = OpenHeap[OpenHeap.Count - 1];
            OpenHeap.RemoveAt(OpenHeap.Count - 1);
            HeapifyFromPosToEnd(0);
            return result;
        }

        public void HeapUpdate(Node node) {
            int pos = -1;
            for (int i = 0; i < OpenHeap.Count; i++) {
                if (OpenHeap[i] == node) {
                    pos = i;
                    break;
                }
            }
            HeapifyFromPosToStart(pos);
        }

        public void HeapifyFromPosToEnd(int pos) {
            while (true) {
                int num = pos;
                int num2 = 2 * pos + 1;
                int num3 = 2 * pos + 2;
                if (num2 < OpenHeap.Count
                    && OpenHeap[num2].F < OpenHeap[num].F) {
                    num = num2;
                }
                if (num3 < OpenHeap.Count
                    && OpenHeap[num3].F < OpenHeap[num].F) {
                    num = num3;
                }
                if (num != pos) {
                    Node value = OpenHeap[num];
                    OpenHeap[num] = OpenHeap[pos];
                    OpenHeap[pos] = value;
                    pos = num;
                    continue;
                }
                break;
            }
        }

        public void HeapifyFromPosToStart(int pos) {
            int num = pos;
            while (num > 0) {
                int num2 = (num - 1) / 2;
                Node node = OpenHeap[num2];
                Node node2 = OpenHeap[num];
                if (node.F > node2.F) {
                    OpenHeap[num2] = node2;
                    OpenHeap[num] = node;
                    num = num2;
                    continue;
                }
                break;
            }
        }

        public Node NewNode(T position, T previousPosition, int g, int h) {
            while (NodesCacheIndex >= NodesCache.Count) {
                NodesCache.Add(new Node());
            }
            Node node = NodesCache[NodesCacheIndex++];
            node.Position = position;
            node.PreviousPosition = previousPosition;
            node.F = g + h;
            node.G = g;
            node.H = h;
            node.IsClosed = false;
            return node;
        }
    }
}